/*  
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建动作服务端；
      3-2.处理请求数据；
      3-3.处理取消任务请求；
      3-4.生成连续反馈。
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "auto_aim_interfaces/action/calib.hpp"
#include "auto_aim_interfaces/msg/gimbal_control.hpp" // 包含GimbalControl消息的定义
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>

#include <opencv2/calib3d.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>

#include <dirent.h>
#include <vector>
#include <string>
#include <cmath>

using namespace std::placeholders;
using auto_aim_interfaces::action::Calib;
using auto_aim_interfaces::msg::GimbalControl;
using GoalHandleCalib = rclcpp_action::ServerGoalHandle<Calib>;
using sensor_msgs::msg::Image;

// 3.定义节点类；
class CalibrationActionServer : public rclcpp::Node
{
public:
  explicit CalibrationActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("calibration_action_server", options),
    action_server_(rclcpp_action::create_server<Calib>(
      this,
      "calibrate_camera",
      std::bind(&CalibrationActionServer::handle_goal, this, _1, _2),
      std::bind(&CalibrationActionServer::handle_cancel, this, _1),
      std::bind(&CalibrationActionServer::handle_accepted, this, _1))),
    chess_w(7),
    chess_h(7),
    cell_w(400),
    cell_h(400),
    read_path("Img_k/"),
    show_res_path(read_path + "/../res_k"),
    criteria(cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 30, 0.001))
  {
    RCLCPP_INFO(this->get_logger(), "动作服务端创建，等待请求...");
    gimbal_control_pub_ = this->create_publisher<GimbalControl>("gimbal_control", 10);
    image_sub_ = this->create_subscription<Image>(
      "image_raw",
      10,
      std::bind(&CalibrationActionServer::imageCallback, this, _1));
    calibration_result_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("calibration_result", 10);
  }
  void triggerSaveImage()
  {
    if (!last_image_.empty()) {
      static int image_counter = 1;
      std::cout<< last_image_.size() <<std::endl;
      std::string filename = read_path +  std::to_string(image_counter++) + ".png";  // 使用索引生成不同的文件名
      std::cout<< cv::imwrite(filename, last_image_) <<std::endl;
      RCLCPP_INFO(this->get_logger(), "保存当前帧到 %s", filename.c_str());
    } else {
      RCLCPP_WARN(this->get_logger(), "没有可保存的图像");
    }
  }

private:
  rclcpp_action::Server<Calib>::SharedPtr action_server_;
  rclcpp::Publisher<GimbalControl>::SharedPtr gimbal_control_pub_; // 创建GimbalControl消息的发布者
  rclcpp::Subscription<Image>::SharedPtr image_sub_; // 图像订阅者
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr calibration_result_pub_;

  // 成员变量
  const int chess_w;
  const int chess_h;
  const int cell_w;
  const int cell_h;
  const std::string read_path;
  const std::string show_res_path;
  const cv::TermCriteria criteria;
  std::vector<cv::Point3f> board; // 棋盘格的3D点集
  std::vector<std::vector<cv::Point3f>> obj_points; // 世界坐标系下的3D点集
  std::vector<std::vector<cv::Point2f>> img_points; // 图像坐标系下的2D点集
  cv::Mat last_image_; // 存储最新的图像帧

  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Calib::Goal> goal);

  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleCalib> goal_handle);

  void execute(const std::shared_ptr<GoalHandleCalib> goal_handle);
  void handle_accepted(const std::shared_ptr<GoalHandleCalib> goal_handle);
  void CalculateWithCircles(const std::shared_ptr<GoalHandleCalib> goal_handle, std::shared_ptr<Calib::Feedback> feedback, std::shared_ptr<Calib::Result> result);
  void setBoard(std::vector<cv::Point3f>& board, int chess_w, int chess_h);
  void controlGimbal(float yaw, float pitch); // 控制云台移动的函数
  void imageCallback(const Image::SharedPtr msg); // 图像回调函数
  void takePicturesAtPoints(const std::vector<std::pair<float, float>>& points, std::shared_ptr<GoalHandleCalib> goal_handle, std::shared_ptr<Calib::Feedback> feedback);

  std::vector<std::string> getFileNames(const std::string& dir_name, bool is_subdir = false);
  std::vector<std::pair<float, float>> generatePoints(float yaw_min, float yaw_max, float pitch_min, float pitch_max, int num_points);
};
// 在类外定义成员函数
void CalibrationActionServer::imageCallback(const Image::SharedPtr msg)
{
  cv_bridge::CvImagePtr cv_ptr;
  try {
    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    last_image_ = cv_ptr->image;  // 更新 last_image_
  } catch (cv_bridge::Exception& e) {
    RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
    return;
  }
}
std::vector<std::pair<float, float>> CalibrationActionServer::generatePoints(float yaw_min, float yaw_max, float pitch_min, float pitch_max, int num_points) {
    std::vector<std::pair<float, float>> points;

    // 计算每个方向上的步长
    float yaw_step = (yaw_max - yaw_min) / 7;
    float pitch_step = (pitch_max - pitch_min) / 7;

    // 生成点，yaw和pitch分别均分
    for (int i = 0; i < 8; ++i) {
        float yaw = yaw_min + i * yaw_step;
        for (int j = 0; j < 8; ++j) {
            float pitch = pitch_min + j * pitch_step;
            points.emplace_back(yaw, pitch);
        }
    }

    return points;
}

void CalibrationActionServer::takePicturesAtPoints(const std::vector<std::pair<float, float>>& points, std::shared_ptr<GoalHandleCalib> goal_handle, std::shared_ptr<Calib::Feedback> feedback) {
  for (const auto& point : points) {
    // 控制相机移动到这个点
    controlGimbal(point.first, point.second);

    // 等待相机稳定
    rclcpp::sleep_for(std::chrono::seconds(1));

    // 触发保存图像
    triggerSaveImage();

    // 更新反馈
    feedback->photo_taken++;
    goal_handle->publish_feedback(feedback);
  }
}
void CalibrationActionServer::controlGimbal(float yaw, float pitch)
{
  GimbalControl msg;
  msg.timestamp = 0; // 根据实际情况设置timestamp
  msg.is_get = true;
  msg.is_shoot = false;
  msg.yaw = yaw;
  msg.pitch = pitch;
  gimbal_control_pub_->publish(msg);
}
rclcpp_action::GoalResponse CalibrationActionServer::handle_goal(
  const rclcpp_action::GoalUUID & uuid,
  std::shared_ptr<const Calib::Goal> goal)
{
  (void)uuid;
  RCLCPP_INFO(this->get_logger(), "接收到动作客户端请求，请求类型为 %s", goal->calibration_type.c_str());
  if (goal->calibration_type == "circle") {
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  } else {
    return rclcpp_action::GoalResponse::REJECT;
  }
}

rclcpp_action::CancelResponse CalibrationActionServer::handle_cancel(
  const std::shared_ptr<GoalHandleCalib> goal_handle)
{
  (void)goal_handle;
  RCLCPP_INFO(this->get_logger(), "接收到任务取消请求");
  return rclcpp_action::CancelResponse::ACCEPT;
}

void CalibrationActionServer::execute(const std::shared_ptr<GoalHandleCalib> goal_handle)
{
  RCLCPP_INFO(this->get_logger(), "开始执行任务");
  const auto goal = goal_handle->get_goal();
  auto feedback = std::make_shared<Calib::Feedback>();
  auto result = std::make_shared<Calib::Result>();

  // 假设我们已经知道了相机视野的边界
  float yaw_min = 9.0f, yaw_max = 20.0f;
  float pitch_min = 1.0f, pitch_max = -12.0f;

  // 生成60个点
  std::vector<std::pair<float, float>> points = generatePoints(yaw_min, yaw_max, pitch_min, pitch_max, 60);

  // 遍历这些点并拍照
  takePicturesAtPoints(points, goal_handle, feedback);

  //根据标定类型执行相应的标定函数
  if (goal->calibration_type == "circle") {
    CalculateWithCircles(goal_handle, feedback, result);
  } else {
    RCLCPP_ERROR(this->get_logger(), "未知的标定类型");
    result->success = false;
    goal_handle->succeed(result);
    return;
  }

  // 如果标定成功
  if (rclcpp::ok()) {
    result->success = true;
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "任务完成！");
  }
}

void CalibrationActionServer::handle_accepted(const std::shared_ptr<GoalHandleCalib> goal_handle)
{
  std::thread{std::bind(&CalibrationActionServer::execute, this, _1), goal_handle}.detach();
}
void CalibrationActionServer::CalculateWithCircles(const std::shared_ptr<GoalHandleCalib> goal_handle, std::shared_ptr<Calib::Feedback> feedback, std::shared_ptr<Calib::Result> result)
{
    try {
        // 初始化必要变量
        std::vector<std::string> paths = getFileNames(read_path); // 获取图像路径
        setBoard(board, chess_w, chess_h); // 设置棋盘格模板

        int img_w, img_h;

        // 循环校准
        for (const auto& path : paths) {
            cv::Mat img = cv::imread(path); // 读取图像
            if (img.empty()) {
                std::cerr << "Error: Image not found or unable to read: " << path << std::endl;
                continue;
            }
            img_w = img.size().width; // 图像宽度
            img_h = img.size().height; // 图像高度
            cv::Mat gray;
            cvtColor(img, gray, cv::COLOR_BGR2GRAY); // 转换为灰度图

            // 寻找圆角点和亚像素精度角点
            std::vector<cv::Point2f> corners;
            bool ret = cv::findCirclesGrid(gray, cv::Size(chess_w, chess_h), corners, cv::CALIB_CB_SYMMETRIC_GRID);

            if (ret) { // 如果找到角点
                obj_points.push_back(board); // 将棋盘格模型点添加到容器
                img_points.push_back(corners); // 将图像点添加到容器

                // 绘制角点
                cv::drawChessboardCorners(img, cv::Size(chess_w, chess_h), corners, ret);

                // 显示带有角点的图像
                cv::imshow("findCorners", img);
                cv::imwrite(show_res_path + "/findCorners_" + std::to_string(obj_points.size()) + ".png", img);
                cv::waitKey(40); // 等待一段时间以查看图像
            }
        }

        cv::destroyAllWindows(); // 销毁所有窗口

        // 检查数据的有效性
        std::cout << "Number of object points: " << obj_points.size() << std::endl;
        std::cout << "Number of image points: " << img_points.size() << std::endl;
        std::cout << "Image width: " << img_w << ", Image height: " << img_h << std::endl;

        // 检查每个对象点和图像点的具体内容
        for (size_t i = 0; i < obj_points.size(); ++i) {
            std::cout << "Object points for image " << i << ": " << obj_points[i].size() << std::endl;
            for (size_t j = 0; j < 5 && j < obj_points[i].size(); ++j) {
                std::cout << "  Point " << j << ": (" << obj_points[i][j].x << ", " << obj_points[i][j].y << ", " << obj_points[i][j].z << ")" << std::endl;
            }

            std::cout << "Image points for image " << i << ": " << img_points[i].size() << std::endl;
            for (size_t j = 0; j < 5 && j < img_points[i].size(); ++j) {
                std::cout << "  Point " << j << ": (" << img_points[i][j].x << ", " << img_points[i][j].y << ")" << std::endl;
            }
        }

        // 计算最终的内参矩阵和畸变系数
        std::cout << "Calculating ... ... " << std::endl;

        cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_32F); // 初始内参矩阵
       
        cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_32F); // 初始畸变系数
        

        std::cout << "Camera matrix size: " << cameraMatrix.size() << std::endl;
        
        std::cout << "Distortion coefficients size: " << distCoeffs.size() << std::endl;
        

        std::vector<cv::Mat> rvecs;
        
        std::vector<cv::Mat> tvecs;
        
        double err = cv::calibrateCamera(obj_points, img_points, cv::Size(img_w, img_h), cameraMatrix, distCoeffs, rvecs, tvecs);
        // std::cout << 7 << std::endl;
        // 复制系数到结果中
        std_msgs::msg::Float64MultiArray result_msg;

        // 将内参矩阵转化为一维数组并添加到消息中
        for (int i = 0; i < cameraMatrix.rows; ++i) {
            for (int j = 0; j < cameraMatrix.cols; ++j) {
                result_msg.data.push_back(cameraMatrix.at<double>(i, j));
            }
        }

        // 将畸变系数转化为一维数组并添加到消息中
        for (int i = 0; i < distCoeffs.rows; ++i) {
            result_msg.data.push_back(distCoeffs.at<double>(i, 0));
        }

        // 发布消息
        calibration_result_pub_->publish(result_msg);

        RCLCPP_INFO(this->get_logger(), "内参矩阵和畸变系数已发布");
        RCLCPP_INFO(this->get_logger(), "内参矩阵:");
        for (int i = 0; i < cameraMatrix.rows; ++i) {
            std::ostringstream row;
            for (int j = 0; j < cameraMatrix.cols; ++j) {
                row << cameraMatrix.at<double>(i, j) << " ";
            }
            RCLCPP_INFO(this->get_logger(), row.str().c_str());
        }

        RCLCPP_INFO(this->get_logger(), "畸变系数:");
        for (int i = 0; i < distCoeffs.rows; ++i) {
            RCLCPP_INFO(this->get_logger(), "%f", distCoeffs.at<double>(i, 0));
        }

        // 计算误差
        result->error = err;
        feedback->calibrating = true;
        feedback->photo_num = obj_points.size();
        // 处理了多少照片
        goal_handle->publish_feedback(feedback);
    } catch (const cv::Exception& e) {
        std::cerr << "OpenCV Exception: " << e.what() << std::endl;
    } catch (const std::exception& e) {
        std::cerr << "Standard Exception: " << e.what() << std::endl;
    } catch (...) {
        std::cerr << "Unknown Exception" << std::endl;
    }
}

std::vector<std::string> CalibrationActionServer::getFileNames(const std::string& dir_name, bool is_subdir) {
    DIR* dir_entry;
    struct dirent* dir_path;
    std::vector<std::string> file_names;

    try {
        if ((dir_entry = opendir(dir_name.c_str())) != NULL) { // 打开目录
            while ((dir_path = readdir(dir_entry)) != NULL) { // 遍历目录项
                // 忽略当前目录和父目录
                if (strcmp(".", dir_path->d_name) == 0 || strcmp("..", dir_path->d_name) == 0) {
                    continue;
                }
                // 添加非目录文件
                if (dir_path->d_type != DT_DIR) {
                    file_names.push_back(dir_name + "/" + dir_path->d_name);
                }
                // 如果允许，递归地添加子目录中的文件
                if (is_subdir && dir_path->d_type == DT_DIR) {
                    std::vector<std::string> sub_file_names = getFileNames(dir_name + "/" + dir_path->d_name, true);
                    file_names.insert(file_names.begin(), sub_file_names.begin(), sub_file_names.end());
                }
            }
        }
    } catch (const std::exception& e) {
        std::cerr << "Exception in getFileNames: " << e.what() << std::endl;
    } catch (...) {
        std::cerr << "Unknown Exception in getFileNames" << std::endl;
    }

    closedir(dir_entry); // 关闭目录句柄

    return file_names; // 返回文件名列表
}

void CalibrationActionServer::setBoard(std::vector<cv::Point3f>& board, int chess_w, int chess_h) {
    try {
        for (int i = 0; i < chess_h; i++) {
            for (int j = 0; j < chess_w; j++) {
                cv::Point3f pt;
                pt.x = j * cell_w; // 水平方向上的偏移量
                pt.y = i * cell_h; // 垂直方向上的偏移量
                pt.z = 0; // 假设棋盘格位于Z=0平面上
                board.push_back(pt); // 将点添加到容器中
            }
        }
    } catch (const std::exception& e) {
        std::cerr << "Exception in setBoard: " << e.what() << std::endl;
    } catch (...) {
        std::cerr << "Unknown Exception in setBoard" << std::endl;
    }
}

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc, argv);
  // 4.调用spin函数，并传入节点对象指针；
  auto action_server = std::make_shared<CalibrationActionServer>();
  rclcpp::spin(action_server);
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}